{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#default_exp radar.config_v1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# radar.config_v1\n",
    "\n",
    "The TI 1443/1843 radar firmware accepts a some commands over the serial port to configure the radar waveform.\n",
    "This module parses these commands so that we can interperet and process the raw ADC readings correctly.\n",
    "\n",
    "Futher details on this commands can be found in the [mmWave SDK user guide](https://www.ti.com/tool/MMWAVE-SDK).\n",
    "\n",
    "*Note:* We do not process all commands, but only command that affect the ADC data. The 1843 supports addtional commands for onboard processing. All these are ignored."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#export\n",
    "import logging\n",
    "\n",
    "logger = logging.getLogger()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#export\n",
    "\n",
    "def read_radar_params(filename):\n",
    "    \"\"\"Reads a text file containing serial commands and returns parsed config as a dictionary\"\"\"\n",
    "    with open(filename) as cfg:\n",
    "        iwr_cmds = cfg.readlines()\n",
    "        iwr_cmds = [x.strip() for x in iwr_cmds]\n",
    "        radar_cfg = parse_commands(iwr_cmds)\n",
    "\n",
    "    logger.debug(radar_cfg)\n",
    "    return radar_cfg\n",
    "\n",
    "def parse_commands(commands):\n",
    "    \"\"\"Calls the corresponding parser for each command in commands list\"\"\"\n",
    "    cfg = None\n",
    "    for line in commands:\n",
    "        try:\n",
    "            cmd = line.split()[0]\n",
    "            args = line.split()[1:]\n",
    "            cfg = command_handlers[cmd](args, cfg)\n",
    "        except KeyError:\n",
    "            logger.debug(f'{cmd} is not handled')\n",
    "        except IndexError:\n",
    "            logger.debug(f'line is empty \"{line}\"')\n",
    "    return cfg\n",
    "\n",
    "def dict_to_list(cfg):\n",
    "    \"\"\"Generates commands from config dictionary\"\"\"\n",
    "    cfg_list = ['flushCfg','dfeDataOutputMode 1']\n",
    "\n",
    "    # rx antennas/lanes for channel config\n",
    "    rx_bool = [cfg['rx4'], cfg['rx3'], cfg['rx2'], cfg['rx1']]\n",
    "    rx_mask = sum(2 ** i for i, v in enumerate(reversed(rx_bool)) if v)\n",
    "    # number of tx antennas for channel config\n",
    "    tx_bool = [cfg['tx3'], cfg['tx2'], cfg['tx1']]\n",
    "    tx_mask = sum(2 ** i for i, v in enumerate(reversed(tx_bool)) if v)\n",
    "    #print('[NOTE] Azimuth angle can be determined from channel config.') if cfg['tx2'] is True and (cfg['tx1'] or cfg['tx3']) is False else 0\n",
    "    #print('[NOTE] Azimuth angle can be determined from channel config.') if cfg['tx2'] is False and (cfg['tx1'] or cfg['tx3']) is True else 0\n",
    "    #print('[NOTE] Elevation and Azimuth angle can be determined from channel config.') if cfg['tx2'] is True and (cfg['tx1'] or cfg['tx3']) else 0\n",
    "    cfg_list.append('channelCfg %s %s 0' % (rx_mask, tx_mask))  # rx and tx mask\n",
    "\n",
    "    # adc config\n",
    "    if cfg['isComplex'] and cfg['image_band']:\n",
    "        outputFmt = 2\n",
    "        #print('[NOTE] Complex 2x mode, both Imaginary and Real IF spectrum is filtered and sent to ADC, so\\n'\n",
    "        #      '       if Sampling rate is X, ADC data would include frequency spectrum from -X/2 to X/2.')\n",
    "    elif cfg['isComplex'] and not cfg['image_band'] == True:\n",
    "        outputFmt = 1\n",
    "        #print('[NOTE] Complex 1x mode, Only Real IF Spectrum is filtered and sent to ADC, so if Sampling rate\\n'\n",
    "        #      '       is X, ADC data would include frequency spectrum from 0 to X.')\n",
    "    else: raise ValueError(\"Real Data Type Not Supported\")\n",
    "    cfg_list.append('adcCfg 2 %s' % outputFmt)  # 16 bits (mandatory), complex 1x or 2x\n",
    "\n",
    "    # adc power\n",
    "    if cfg['adcPower'] =='low':\n",
    "        power_mode = 1\n",
    "        #print('[NOTE] The Low power ADC mode limits the sampling rate to half the max value.')\n",
    "    elif cfg['adcPower'] =='regular': power_mode = 0\n",
    "    else: raise ValueError(\"ADC power level Not Supported\")\n",
    "    cfg_list.append('lowPower 0 %s' % power_mode)  # power mode\n",
    "\n",
    "    # profile configs\n",
    "    for profile_ii in cfg['profiles']:\n",
    "        cfg_list.append('profileCfg %s %s %s %s %s %s %s %s %s %s %s %s %s %s'\n",
    "                % (profile_ii['id'],\n",
    "                float(profile_ii['start_frequency']/1e9),\n",
    "                float(profile_ii['idle']/1e-6),\n",
    "                float(profile_ii['adcStartTime']/1e-6),\n",
    "                float(profile_ii['rampEndTime']/1e-6),\n",
    "                int(profile_ii['txPower']),\n",
    "                int(profile_ii['txPhaseShift']),\n",
    "                float(profile_ii['freqSlopeConst']/1e12),\n",
    "                float(profile_ii['txStartTime']/1e-6),\n",
    "                int(profile_ii['adcSamples']),\n",
    "                int(profile_ii['adcSampleRate']/1e3),\n",
    "                int(profile_ii['hpfCornerFreq1']),\n",
    "                int(profile_ii['hpfCornerFreq2']),\n",
    "                int(profile_ii['rxGain'])))\n",
    "\n",
    "    # chirp configs\n",
    "    for chirp_ii in cfg['chirps']:\n",
    "\n",
    "        # Check if chirp is referring to valid profile config\n",
    "        profile_valid = False\n",
    "        for profile_ii in cfg['profiles']:\n",
    "            if chirp_ii['profileID'] == profile_ii['id']: profile_valid = True\n",
    "        if profile_valid is False: raise ValueError(\"The following profile id used in chirp \"\n",
    "                                                    \"is invalid: %i\" % chirp_ii['profileID'])\n",
    "        ###############################################################################################################\n",
    "        '''\n",
    "        # check if tx values are valid\n",
    "        if hamming([chirp_ii['chirptx3'],chirp_ii['chirptx2'],chirp_ii['chirptx1']],\n",
    "            [cfg['tx3'], cfg['tx2'], cfg['tx1']])*3 > 1:\n",
    "            raise ValueError(\"Chirp should have at most one different Tx than channel cfg\")\n",
    "        '''\n",
    "        ###############################################################################################################\n",
    "        if chirp_ii['chirpStartIndex'] > chirp_ii['chirpStopIndex']: raise ValueError(\"Particular chirp start index after chirp stop index\")\n",
    "        tx_bool = [chirp_ii['chirptx3'],chirp_ii['chirptx2'],chirp_ii['chirptx1']]\n",
    "        tx_mask = sum(2 ** i for i, v in enumerate(reversed(tx_bool)) if v)\n",
    "        cfg_list.append('chirpCfg %s %s %s %s %s %s %s %s'\n",
    "                % (chirp_ii['chirpStartIndex'],\n",
    "                   chirp_ii['chirpStopIndex'],\n",
    "                   chirp_ii['profileID'],\n",
    "                   chirp_ii['startFreqVariation'],\n",
    "                   chirp_ii['slopeVariation'],\n",
    "                   chirp_ii['idleVariation'],\n",
    "                   chirp_ii['adcStartVariation'],\n",
    "                   tx_mask))\n",
    "\n",
    "    # frame config\n",
    "    chirpStop = 0\n",
    "    chirpStart = 511  # max value for chirp start index\n",
    "    for chirp_ii in cfg['chirps']:\n",
    "        chirpStop = max(chirpStop, chirp_ii['chirpStopIndex'])\n",
    "        chirpStart = min(chirpStart,chirp_ii['chirpStartIndex'])\n",
    "    chirps_len  = chirpStop + 1\n",
    "\n",
    "    numLoops = cfg['numChirps']/chirps_len\n",
    "    if chirpStart > chirpStop: raise ValueError(\"Chirp(s) start index is after chirp stop index\")\n",
    "    if numLoops % 1 != 0: raise ValueError(\"Number of loops is not integer\")\n",
    "    if numLoops > 255 or numLoops < 1: raise ValueError(\"Number of loops must be int in [1,255]\")\n",
    "\n",
    "    numFrames = cfg['numFrames'] if 'numFrames' in cfg.keys() else 0  # if zero => inf\n",
    "\n",
    "    cfg_list.append('frameCfg %s %s %s %s %s 1 0'\n",
    "            % (chirpStart, chirpStop, int(numLoops), numFrames, 1000/cfg['fps']))\n",
    "\n",
    "    cfg_list.append('testFmkCfg 0 0 0 1')\n",
    "    cfg_list.append('setProfileCfg disable ADC disable')\n",
    "    return cfg_list\n",
    "\n",
    "\n",
    "def channelStr_to_dict(args, curr_cfg=None):\n",
    "    \"\"\"Handler for `channelcfg`\"\"\"\n",
    "\n",
    "    if curr_cfg:\n",
    "        cfg = curr_cfg\n",
    "    else:\n",
    "        cfg = {}\n",
    "\n",
    "    # This is the number of receivers which is equivalent to the number of lanes in the source code\n",
    "    # Later, may include the result from the number of transmitters\n",
    "    rx_bin = bin(int(args[0]))[2:].zfill(4)\n",
    "    cfg['numLanes'] = len([ones for ones in rx_bin if ones == '1'])\n",
    "    (cfg['rx4'],cfg['rx3'],cfg['rx2'],cfg['rx1']) = [bool(int(ones)) for ones in rx_bin]\n",
    "\n",
    "    # This is the number of transmitters\n",
    "    tx_bin = bin(int(args[1]))[2:].zfill(3)\n",
    "    cfg['numTx'] = len([ones for ones in tx_bin if ones == '1'])\n",
    "    (cfg['tx3'], cfg['tx2'], cfg['tx1']) = [bool(int(ones)) for ones in tx_bin]\n",
    "    #print('[NOTE] Azimuth angle can be determined from channel config.') if cfg['tx2'] is True and (cfg['tx1'] or cfg['tx3']) is False else 0\n",
    "    #print('[NOTE] Azimuth angle can be determined from channel config.') if cfg['tx2'] is False and (cfg['tx1'] or cfg['tx3']) is True else 0\n",
    "    #print('[NOTE] Elevation and Azimuth angle can be determined from channel config.') if cfg['tx2'] is True and (cfg['tx1'] or cfg['tx3']) else 0\n",
    "\n",
    "\n",
    "    return cfg\n",
    "\n",
    "\n",
    "def profileStr_to_dict(args, curr_cfg=None):\n",
    "    \"\"\"Handler for `profileCfg`\"\"\"\n",
    "    normalizer = [None, 1e9, 1e-6, 1e-6, 1e-6, None, None, 1e12, 1e-6, None, 1e3, None, None, None]\n",
    "    dtype = [int, float, float, float, float, float, float, float, float, int, float, int, int, float]\n",
    "    keys = ['id',\n",
    "            'start_frequency',\n",
    "            'idle',\n",
    "            'adcStartTime',\n",
    "            'rampEndTime',\n",
    "            'txPower',\n",
    "            'txPhaseShift',\n",
    "            'freqSlopeConst',\n",
    "            'txStartTime',\n",
    "            'adcSamples',\n",
    "            'adcSampleRate',\n",
    "            'hpfCornerFreq1',\n",
    "            'hpfCornerFreq2',\n",
    "            'rxGain',\n",
    "            ]\n",
    "    # Check if the main dictionary exists\n",
    "    if curr_cfg:\n",
    "        cfg = curr_cfg\n",
    "        if 'profiles' not in cfg.keys():\n",
    "            cfg['profiles']=[]\n",
    "    else:\n",
    "        cfg = {'profiles': []}\n",
    "\n",
    "    profile_dict = {}\n",
    "    for k, v, n, d in zip(keys, args, normalizer, dtype):\n",
    "        profile_dict[k] = d(float(v) * n if n else v)\n",
    "\n",
    "    cfg['profiles'].append(profile_dict)\n",
    "    return cfg\n",
    "\n",
    "\n",
    "def chirp_to_dict(args,curr_cfg=None):\n",
    "    \"\"\"Handler for `chirpCfg`\"\"\"\n",
    "    if curr_cfg:\n",
    "        cfg = curr_cfg\n",
    "        if 'chirps' not in cfg.keys():\n",
    "            cfg['chirps'] = []\n",
    "    else:\n",
    "        cfg = {'chirps': []}\n",
    "\n",
    "    chirp_dict = {}\n",
    "    chirp_dict['chirpStartIndex'] = int(args[0])\n",
    "    chirp_dict['chirpStopIndex'] = int(args[1])\n",
    "    chirp_dict['profileID'] = int(args[2])\n",
    "    chirp_dict['startFreqVariation'] = float(args[3])\n",
    "    chirp_dict['slopeVariation'] = float(args[4])\n",
    "    chirp_dict['idleVariation'] = float(args[5])\n",
    "    chirp_dict['adcStartVariation'] = float(args[6])\n",
    "\n",
    "    tx_bin = bin(int(args[7]))[2:].zfill(3)\n",
    "    (chirp_dict['chirptx3'], chirp_dict['chirptx2'], chirp_dict['chirptx1']) = [bool(int(ones)) for ones in tx_bin]\n",
    "\n",
    "    cfg['chirps'].append(chirp_dict)\n",
    "    return cfg\n",
    "\n",
    "\n",
    "def power_to_dict(args,curr_cfg=None):\n",
    "    \"\"\"handler for `lowPower`\"\"\"\n",
    "    if curr_cfg:\n",
    "        cfg = curr_cfg\n",
    "    else:\n",
    "        cfg = {}\n",
    "    if int(args[1]) ==1:\n",
    "        cfg['adcPower'] = 'low'\n",
    "        #print('[NOTE] The Low power ADC mode limits the sampling rate to half the max value.')\n",
    "    elif int(args[1]) ==0:\n",
    "        cfg['adcPower'] = 'regular'\n",
    "    else:\n",
    "        raise ValueError (\"Invalid Power Level\")\n",
    "    return cfg\n",
    "\n",
    "\n",
    "def frameStr_to_dict(args, cfg):\n",
    "    \"\"\"Handler for `frameCfg`\"\"\"\n",
    "\n",
    "    # Number of chirps\n",
    "    if 'chirps' not in cfg.keys():\n",
    "        raise ValueError(\"Need to define chirps before frame\")\n",
    "\n",
    "    chirpStop =0\n",
    "    for ii in range(len(cfg['chirps'])):\n",
    "        chirpStop = max(chirpStop,cfg['chirps'][ii]['chirpStopIndex'])\n",
    "    chirps_len = chirpStop + 1\n",
    "\n",
    "    cfg['numChirps'] = int(args[2]) * chirps_len  # num loops * len(chirps)\n",
    "    if int(args[3]) != 0: cfg['numFrames'] = int(args[3])\n",
    "\n",
    "    # args[4] is the time in milliseconds of each frame\n",
    "    cfg['fps'] = 1000/float(args[4])\n",
    "\n",
    "\n",
    "    return cfg\n",
    "\n",
    "\n",
    "def adcStr_to_dict(args, curr_cfg=None):\n",
    "    \"\"\"Handler for `adcCfg`\"\"\"\n",
    "    if curr_cfg:\n",
    "        cfg = curr_cfg\n",
    "    else:\n",
    "        cfg = {}\n",
    "\n",
    "    if int(args[1]) == 1:\n",
    "        cfg['isComplex'] = True\n",
    "        cfg['image_band'] = False\n",
    "        #print('[NOTE] Complex 1x mode, Only Real IF Spectrum is filtered and sent to ADC, so if Sampling rate\\n'\n",
    "        #      '       is X, ADC data would include frequency spectrum from 0 to X.')\n",
    "    elif int(args[1]) == 2:\n",
    "        cfg['isComplex'] = True\n",
    "        cfg['image_band'] = True\n",
    "        #print('[NOTE] Complex 2x mode, both Imaginary and Real IF spectrum is filtered and sent to ADC, so\\n'\n",
    "        #      '       if Sampling rate is X, ADC data would include frequency spectrum from -X/2 to X/2.')\n",
    "    else:\n",
    "        raise ValueError(\"Real Data Type Not Supported\")\n",
    "\n",
    "    return cfg\n",
    "\n",
    "#Mapping of serial command to command handler\n",
    "command_handlers = {\n",
    "    'channelCfg': channelStr_to_dict,\n",
    "    'profileCfg': profileStr_to_dict,\n",
    "    'chirpCfg': chirp_to_dict,\n",
    "    'frameCfg': frameStr_to_dict,\n",
    "    'adcCfg': adcStr_to_dict,\n",
    "    'lowPower': power_to_dict,\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Example Usage\n",
    "\n",
    "For a text file with the following commands:\n",
    "```\n",
    "flushCfg\n",
    "dfeDataOutputMode 1\n",
    "channelCfg 15 5 0\n",
    "adcCfg 2 1\n",
    "lowPower 0 0\n",
    "profileCfg 0 77.0 58.0 7.0 40.0 0 0 100.0 1.0 304 9499 0 0 30\n",
    "chirpCfg 0 0 0 0.0 0.0 0.0 0.0 1\n",
    "chirpCfg 1 1 0 0.0 0.0 0.0 0.0 4\n",
    "frameCfg 0 1 32 0 33.333 1 0\n",
    "testFmkCfg 0 0 0 1\n",
    "setProfileCfg disable ADC disable\n",
    "sensorStart\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#hide\n",
    "radar_config_filename = '../samples/indoor_human_rcs.cfg'"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'numLanes': 4,\n",
       " 'rx4': True,\n",
       " 'rx3': True,\n",
       " 'rx2': True,\n",
       " 'rx1': True,\n",
       " 'numTx': 2,\n",
       " 'tx3': True,\n",
       " 'tx2': False,\n",
       " 'tx1': True,\n",
       " 'isComplex': True,\n",
       " 'image_band': False,\n",
       " 'profiles': [{'id': 0,\n",
       "   'start_frequency': 77000000000.0,\n",
       "   'idle': 5.8e-05,\n",
       "   'adcStartTime': 7e-06,\n",
       "   'rampEndTime': 3.9999999999999996e-05,\n",
       "   'txPower': 0.0,\n",
       "   'txPhaseShift': 0.0,\n",
       "   'freqSlopeConst': 100000000000000.0,\n",
       "   'txStartTime': 1e-06,\n",
       "   'adcSamples': 304,\n",
       "   'adcSampleRate': 9499000.0,\n",
       "   'hpfCornerFreq1': 0,\n",
       "   'hpfCornerFreq2': 0,\n",
       "   'rxGain': 30.0}],\n",
       " 'chirps': [{'chirpStartIndex': 0,\n",
       "   'chirpStopIndex': 0,\n",
       "   'profileID': 0,\n",
       "   'startFreqVariation': 0.0,\n",
       "   'slopeVariation': 0.0,\n",
       "   'idleVariation': 0.0,\n",
       "   'adcStartVariation': 0.0,\n",
       "   'chirptx3': False,\n",
       "   'chirptx2': False,\n",
       "   'chirptx1': True},\n",
       "  {'chirpStartIndex': 1,\n",
       "   'chirpStopIndex': 1,\n",
       "   'profileID': 0,\n",
       "   'startFreqVariation': 0.0,\n",
       "   'slopeVariation': 0.0,\n",
       "   'idleVariation': 0.0,\n",
       "   'adcStartVariation': 0.0,\n",
       "   'chirptx3': True,\n",
       "   'chirptx2': False,\n",
       "   'chirptx1': False}],\n",
       " 'numChirps': 64,\n",
       " 'fps': 30.00030000300003,\n",
       " 'adcPower': 'regular'}"
      ]
     },
     "execution_count": null,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "read_radar_params(radar_config_filename)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
